package ch.ethz.fcl.process.polygon;

import java.util.ArrayList;
import java.util.Collections;
import java.util.Comparator;
import java.util.HashMap;
import java.util.List;
import java.util.Map;

import org.opencv.core.MatOfPoint;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.Point;
import org.opencv.imgproc.Imgproc;

import ch.ethz.fcl.Constant;
import ch.ethz.fcl.KinectContainer;
import ch.ethz.fcl.input.Coordinate;
import ch.ethz.fcl.tools.Tools;

import com.seisw.util.geom.Clip;
import com.seisw.util.geom.Poly;
import com.seisw.util.geom.PolyDefault;

public class PolygonFit {
	private KinectContainer container;

	static double totalPolyLength = 0;
	private List<Polygon> polygons = null;
	private List<Line> originLines = null;

	public PolygonFit(KinectContainer container) {
		this.container = container;
	}

	public List<MatOfPoint> newProcessPolys(int fixedHeight,
			List<MatOfPoint> shapes) {
		List<MatOfPoint> results = new ArrayList<MatOfPoint>();

		polygons = new ArrayList<Polygon>();
		for (int i = 0; i < shapes.size(); i++) {
			if (Constant.DEBUG)
				if (i != 6)
					continue;

			MatOfPoint shape = shapes.get(i);
			if (shape == null)
				continue;

			totalPolyLength = Imgproc.arcLength(
					new MatOfPoint2f(shape.toArray()), true);

			originLines = generateLines(shape);
			if (originLines.size() == 0 || originLines.size() == 1)
				continue;

			List<Line> sortedLines = sortLines(originLines);

			if (Constant.DEBUG)
				for (Line l : originLines) {
					results.add(l.toMatOfPoint());
				}

			possiblePolygons(sortedLines);
			List<Polygon> conductedPolygons = new ArrayList<Polygon>();

			Map<Line, Line> lineMap = new HashMap<Line, Line>();
			mapLines(lineMap);

			Polygon conductedPolygon = this.generateRectangle(container,
					fixedHeight, originLines, lineMap);
			conductedPolygons.add(conductedPolygon);

			polygons.add(conductedPolygon);
		}

		polygons = removeDuplicatePolygons(polygons);
		polygons = removeSidePolygons(polygons);
		polygons = removeOutTablePolygons(polygons);

		polygons = sortPolygonsAntiClockwise(polygons);

		for (Polygon polygon : polygons) {
			polygon.setHeight(fixedHeight);

			MatOfPoint polygonMatPoint = polygon.getConductedPolygon()
					.toMatOfPoint();
			if (polygonMatPoint == null)
				continue;

			if (!Constant.DEBUG)
				results.add(polygonMatPoint);
		}

		return results;
	}

	public List<MatOfPoint> processPolys(int fixedHeight,
			List<MatOfPoint> shapes) {
		List<MatOfPoint> results = new ArrayList<MatOfPoint>();

		polygons = new ArrayList<Polygon>();
		for (int i = 0; i < shapes.size(); i++) {
			// if (i != 4)
			// continue;

			MatOfPoint shape = shapes.get(i);
			if (shape == null)
				continue;

			totalPolyLength = Imgproc.arcLength(
					new MatOfPoint2f(shape.toArray()), true);

			originLines = generateLines(shape);
			if (originLines.size() == 0 || originLines.size() == 1)
				continue;

			List<Line> sortedLines = sortLines(originLines);

			if (Constant.DEBUG)
				for (Line l : originLines) {
					results.add(l.toMatOfPoint());
				}

			List<Polygon> possiblePolygons = possiblePolygons(sortedLines);
			List<Polygon> conductedPolygons = new ArrayList<Polygon>();

			Map<Line, Line> lineMap = new HashMap<Line, Line>();
			mapLines(lineMap);

			for (int index = 0; index < possiblePolygons.size(); index++) {
				// if(index != 0)
				// continue;

				if (Constant.DEBUG)
					System.out.println("\nFor polygons  " + index);

				Polygon polygon = possiblePolygons.get(index);
				// for (Line p : rec.originLines)
				// results.add(p.toMatOfPoint());

				Polygon conductedPolygon = polygon.generateRectangle(
						fixedHeight, container, originLines);

				if (conductedPolygon == null
						|| conductedPolygon.getPoints() == null
						|| conductedPolygon.getPoints().size() <= 2)
					continue;

				boolean overlapping = false;
				for (int k = 0; k < conductedPolygons.size(); k++) {
					double intersectRatio = polygonIntersection(
							conductedPolygon.getPoints(), conductedPolygons
									.get(k).getPoints());
					if (intersectRatio > 0.5) {
						overlapping = true;
						break;
					}
				}

				if (overlapping)
					continue;

				if (polygon.getHeight() >= fixedHeight
						+ Constant.HEIGHT_VARIANCE
						|| polygon.getHeight() <= fixedHeight
								- Constant.HEIGHT_VARIANCE)
					continue;

				conductedPolygons.add(conductedPolygon);
				polygons.add(polygon);
			}
		}

		polygons = splitPolygons(polygons);
		polygons = removeDuplicatePolygons(polygons);
		polygons = removeSidePolygons(polygons);
		polygons = removeOutTablePolygons(polygons);

		polygons = sortPolygonsAntiClockwise(polygons);

		for (Polygon polygon : polygons) {
			polygon.setHeight(fixedHeight);

			MatOfPoint polygonMatPoint = polygon.getConductedPolygon()
					.toMatOfPoint();
			if (polygonMatPoint == null)
				continue;

			if (!Constant.DEBUG)
				results.add(polygonMatPoint);
		}

		return results;
	}

	private List<Polygon> splitPolygons(List<Polygon> polygons) {
		List<Polygon> splitPolygons = new ArrayList<Polygon>();
		for (Polygon polygon : polygons) {
			for (Polygon splitPolygon : polygon.getSplitConductedPolygon()) {
				if (splitPolygon == null || splitPolygon.getPoints() == null)
					continue;
				if (splitPolygon.getPoints().size() == 0)
					continue;
				if (splitPolygon.getHeight() == 0)
					continue;
				splitPolygons.add(splitPolygon);
			}
		}
		return splitPolygons;
	}

	private List<Polygon> removeDuplicatePolygons(List<Polygon> polygons) {
		List<Polygon> nonDuplicatePolygons = new ArrayList<Polygon>();
		for (Polygon polygon : polygons) {
			if (polygon == null)
				continue;
			if (polygon.getPoints().size() == 0)
				continue;

			boolean exist = false;
			for (Polygon existPolygon : nonDuplicatePolygons)
				if (polygonIntersection(polygon.getPoints(),
						existPolygon.getPoints()) > 0.6f) {
					exist = true;
					break;
				}
			if (!exist) {
				nonDuplicatePolygons.add(polygon);
			}
		}

		return nonDuplicatePolygons;
	}

	// some polygons are created because of the side effect of high boxes, they
	// are not wanted
	private List<Polygon> removeSidePolygons(List<Polygon> polygons) {
		List<Polygon> topPolygons = new ArrayList<Polygon>();
		for (Polygon polygon : polygons) {
			if (polygon.getPoints().size() < 4)
				continue;

			double area = Imgproc.contourArea(polygon.toMatOfPoint());

			if (area < Constant.MIN_CONTOUR_SIZE
					&& polygon.getHeight() < 100 - Constant.HEIGHT_VARIANCE)
				continue;

			List<Point> pts = polygon.getPoints();
			boolean concave = false;
			double zcrossRecord = 1;
			for (int i = 0; i < pts.size(); i++) {
				Point p0 = pts.get(i);
				Point p1 = pts.get((i + 1) % pts.size());
				Point p2 = pts.get((i + 2) % pts.size());

				double dx1 = p1.x - p0.x;
				double dy1 = p1.y - p0.y;
				double dx2 = p2.x - p1.x;
				double dy2 = p2.y - p1.y;
				double zcrossproduct = dx1 * dy2 - dy1 * dx2;
				if (i == 0)
					zcrossRecord = zcrossproduct;
				else if (zcrossRecord * zcrossproduct < 0) {
					concave = true;
					break;
				} else
					zcrossRecord = zcrossproduct;
			}

			// if concave make minimum contour size twice
			if (concave && area < Constant.MIN_CONTOUR_SIZE * 2)
				continue;

			topPolygons.add(polygon);
		}
		return topPolygons;
	}

	// some polygons are out of table created by other objects
	List<Polygon> removeOutTablePolygons(List<Polygon> polygons) {
		List<Polygon> inTablePolygons = new ArrayList<Polygon>();
		for (Polygon polygon : polygons) {
			int onTablePointsSize = 0;
			List<Point> pts = polygon.getPoints();

			for (int j = 0; j < pts.size(); j++) {
				if (Coordinate.pointValid(pts.get(j)))
					onTablePointsSize++;
			}

			if (onTablePointsSize == pts.size())
				inTablePolygons.add(polygon);
		}
		return inTablePolygons;
	}

	private List<Polygon> sortPolygonsAntiClockwise(List<Polygon> polygons) {
		List<Polygon> sortedPolygons = new ArrayList<Polygon>();

		for (Polygon polygon : polygons) {
			Polygon sortedPolygon = sortAntiClockwise(polygon);

			if (sortedPolygon != null) {
				sortedPolygon.setHeight(polygon.height);
				sortedPolygons.add(sortedPolygon);
			}
		}

		return sortedPolygons;
	}

	/**
	 * sort the input polygon points into anti-clockwise order ref:
	 * http://stackoverflow
	 * .com/questions/1165647/how-to-determine-if-a-list-of-polygon
	 * -points-are-in-clockwise-order
	 * 
	 * @param polygon
	 * @return
	 */
	private Polygon sortAntiClockwise(Polygon polygon) {
		if (polygon.originPoints == null || polygon.originPoints.size() <= 2)
			return null;
		int size = polygon.originPoints.size();
		Polygon orderedP = new Polygon();
		boolean isAntiClockwise = isAntiClockwise(polygon.originPoints);
		for (int i = 0; i < size; i++) {
			if (isAntiClockwise) {
				orderedP.addPoints(polygon.originPoints.get(i));
			} else
				orderedP.addPoints(polygon.originPoints.get(size - 1 - i));
		}

		return orderedP;
	}

	private boolean isAntiClockwise(List<Point> pts) {
		float value = 0;
		for (int i = 0; i < pts.size(); i++) {
			Point p0 = pts.get(i);
			Point p1 = pts.get((i + 1) % pts.size());

			value += (p1.x - p0.x) / (p1.y + p0.y);
		}

		return value < 0;
	}

	/**
	 * check how much two polygons intersect with each other, hence to determine
	 * whether keep both of them
	 * 
	 * @param recPts1
	 * @param recPts2
	 * @return percentage of overlapping area to the
	 */
	private double polygonIntersection(List<Point> recPts1, List<Point> recPts2) {
		Poly p1 = new PolyDefault();
		for (Point p : recPts1)
			p1.add(p.x, p.y);

		Poly p2 = new PolyDefault();
		for (Point p : recPts2)
			p2.add(p.x, p.y);

		Poly intersect = Clip.intersection(p1, p2);

		double intersectArea = intersect.getArea();

		return Math.max(intersectArea / p1.getArea(),
				intersectArea / p2.getArea());
	}

	/**
	 * sort ploy lines descending according to their length
	 * 
	 * @param lines
	 * @return
	 */
	List<Line> sortLines(List<Line> lines) {
		List<Line> orderedPolys = new ArrayList<Line>();

		orderedPolys.addAll(lines);

		Collections.sort(orderedPolys, new Comparator<Line>() {
			@Override
			public int compare(Line l1, Line l2) {
				float length1 = l1.length();
				float length2 = l2.length();
				if (length1 > length2)
					return -1;
				else if (length1 < length2)
					return 1;

				return 0;
			}
		});
		return orderedPolys;
	}

	/**
	 * covert the points into straight lines
	 * 
	 * @param polyPoints
	 * @return
	 */
	List<Line> generateLines(MatOfPoint polyPoints) {
		List<Line> lines = new ArrayList<Line>();
		Point[] points = polyPoints.toArray();

		for (int i = 0; i < points.length; i++) {
			Point p1 = points[i];
			Point p2 = points[(i + 1) % points.length];

			Line line = new Line(p1, p2);

			// System.out.println("line " + i + " angle " + line.angleDegree);
			lines.add(line);
		}

		return lines;
	}

	/**
	 * return possible polygons
	 * 
	 * @param sortedLines
	 * @return
	 */
	List<Polygon> possiblePolygons(List<Line> sortedLines) {
		int size = sortedLines.size();
		List<Polygon> polygons = new ArrayList<Polygon>();

		for (int i = 0; i < size; i++) {
			Line line = sortedLines.get(i);
			float angle = line.angleDegree();
			float length = line.length();

			// ignore short lines
			if (length < Constant.MIN_LINE_LENGTH)
				continue;

			if (Constant.DEBUG)
				System.out.println("line angle " + line.angleDegree);
			Polygon fitPolygon = closestPolygon(polygons, angle, length);

			if (fitPolygon == null) {
				fitPolygon = new Polygon();
				fitPolygon.addPolyLine(line);
				polygons.add(fitPolygon);
			} else {
				fitPolygon.addPolyLine(line);
			}
		}

		Collections.sort(polygons, new Comparator<Polygon>() {
			@Override
			public int compare(Polygon o1, Polygon o2) {
				if (o1.length > o2.length)
					return -1;
				else if (o1.length < o2.length)
					return 1;

				return 0;
			}
		});

		List<Polygon> possiblePolygons = new ArrayList<Polygon>();
		for (int i = 0; i < polygons.size(); i++) {
			// System.out.println("possible rectangle is "
			// + polygons.get(i).prinAxis + " length is "
			// + polygons.get(i).length + " poly length "
			// + totalPolyLength);

			// if angle weight is large than 20% of total length
			if (polygons.get(i).length > 50)
				possiblePolygons.add(polygons.get(i));
		}

		for (Polygon possibleRec : possiblePolygons) {
			// add possible angles but not added lines into the rectangle
			for (Line l : sortedLines)
				if (!possibleRec.originLines.contains(l)) {
					double angleDiff = Tools.angleDiff(possibleRec.prinAxis,
							l.angleDegree);

					if (angleDiff < Constant.ANGLE_THRE) {
						possibleRec.addPolyLine(l);
					}
				}

			List<Line> removedLines = new ArrayList<Line>();
			// remove lines that should not be in the rectangle
			for (Line l : possibleRec.originLines) {
				// check both the previous and next line if they belongs to
				// the same polygon that different from current one
				int index = originLines.indexOf(l);

				// System.out.println("" + index + " line length " +
				// l.length());
				Line preLine = originLines.get((index - 1 + size) % size);
				Line nextLine = originLines.get((index + 1 + size) % size);

				if (Constant.DEBUG)
					if (index == 0)
						System.out.println("pre line length "
								+ preLine.length() + " this line length "
								+ l.length() + " next line length "
								+ nextLine.length());

				if (possibleRec.originLines.contains(preLine)
						|| possibleRec.originLines.contains(nextLine))
					continue;

				if (preLine.length() > l.length() * 2
						&& !possibleRec.originLines.contains(preLine))
					removedLines.add(l);
				else if (nextLine.length() > l.length() * 2
						&& !possibleRec.originLines.contains(nextLine))
					removedLines.add(l);
			}

			possibleRec.originLines.removeAll(removedLines);
		}

		// remove all lines already exist in previous polygons
		for (int i = 1; i < possiblePolygons.size(); i++) {
			Polygon pi = possiblePolygons.get(i);
			List<Line> removedLines = new ArrayList<Line>();
			for (int j = 0; j < i; j++) {
				Polygon pj = possiblePolygons.get(j);
				removedLines.addAll(pj.originLines);
			}
			pi.originLines.removeAll(removedLines);
		}

		// for lines connecting two polygons and not added to any possible
		// polygons yet,
		// choose the closest polygon for it
		if (possiblePolygons.size() >= 2)
			for (int i = 0; i < originLines.size(); i++) {
				Line line = originLines.get(i);
				if (line.length() < Constant.MIN_LINE_LENGTH)
					continue;
				int thisLinePolygonIndex = lineInPolygonIndex(possiblePolygons,
						line);
				if (thisLinePolygonIndex != -1)
					continue;

				Line nextLine = originLines.get((i + 1) % originLines.size());
				int nextLinePolygonIndex = lineInPolygonIndex(possiblePolygons,
						nextLine);
				if (nextLinePolygonIndex == -1)
					continue;

				Line preLine = originLines.get((i - 1 + originLines.size())
						% originLines.size());
				int preLinePolygonIndex = lineInPolygonIndex(possiblePolygons,
						preLine);
				if (preLinePolygonIndex == -1)
					continue;

				if (preLinePolygonIndex == nextLinePolygonIndex)
					continue;

				double prePolygonAngleDiff = Tools.angleDiff(
						possiblePolygons.get(preLinePolygonIndex).prinAxis,
						line.angleDegree());
				double nextPolygonAngleDiff = Tools.angleDiff(
						possiblePolygons.get(nextLinePolygonIndex).prinAxis,
						line.angleDegree());
				if (prePolygonAngleDiff > nextPolygonAngleDiff)
					possiblePolygons.get(nextLinePolygonIndex)
							.addPolyLine(line);
				else
					possiblePolygons.get(preLinePolygonIndex).addPolyLine(line);
			}

		for (Polygon p : possiblePolygons)
			for (Line l : p.originLines)
				l.prinAxis = p.prinAxis;

		if (Constant.DEBUG)
			System.out.println("possible polygons size "
					+ possiblePolygons.size());

		return possiblePolygons;
	}

	private int lineInPolygonIndex(List<Polygon> polygons, Line line) {
		int polygonIndex = -1;
		for (int j = 0; j < polygons.size(); j++) {
			if (polygons.get(j).originLines.contains(line)) {
				polygonIndex = j;
				break;
			}
		}
		return polygonIndex;
	}

	// select closest polygon fitting the angle in the rectangle list
	Polygon closestPolygon(List<Polygon> polygon, double theta, float length) {
		Polygon fitRectangle = null;
		double minAngleDiff = Double.MAX_VALUE;
		int minAngleIndex = -1;

		for (int i = 0; i < polygon.size(); i++) {
			double angleDiff = Tools.angleDiff(polygon.get(i).prinAxis, theta);

			if (angleDiff < minAngleDiff) {
				minAngleDiff = angleDiff;
				minAngleIndex = i;
			}
		}

		if (minAngleIndex != -1) {
			fitRectangle = polygon.get(minAngleIndex);
		}

		return fitRectangle;
	}

	/**
	 * put poly-lines that should be one line into one line and put them into
	 * line map
	 * 
	 * @return
	 */
	private void mapLines(Map<Line, Line> lineMap) {
		List<Line> sameLines = new ArrayList<Line>();

		for (int i = 0; i < originLines.size(); i++) {
			Line line = originLines.get(i);

			if (line.prinAxis == -1)
				continue;

			int sameLineIndex = -1;
			Line sameLine = null;
			for (int j = 0; j < sameLines.size(); j++) {
				Line line2 = sameLines.get(j);

				sameLine = line.sameLine(line2);
				if (sameLine != null) {
					sameLineIndex = j;
					break;
				}
			}
			if (sameLine != null) {
				if (Constant.DEBUG) {
					System.out.println("line " + line.angleDegree + " is same "
							+ sameLine.angleDegree);
					System.out.println("line index " + i + " same line index "
							+ sameLineIndex);
				}
				lineMap.remove(sameLines.get(sameLineIndex));
				lineMap.put(sameLines.get(sameLineIndex), sameLine);
				lineMap.put(line, sameLine);

				sameLines.remove(sameLineIndex);
				sameLines.add(sameLine);
			} else {
				sameLines.add(line);
				lineMap.put(line, line);
			}
		}
	}

	public Polygon generateRectangle(KinectContainer container,
			int fixedHeight, List<Line> originLines, Map<Line, Line> lineMap) {
		Polygon conductedPolygon = new Polygon();

		int startIndex = 0;
		Line line = originLines.get(startIndex);
		while (lineMap.get(line) == null) {
			startIndex = (startIndex + 1) % originLines.size();
			line = originLines.get(startIndex);

			if (startIndex == 0)
				return null;
		}

		// start from start index such that we can take a round even first lines
		// are null
		for (int i = startIndex; i < originLines.size() + startIndex; i++) {
			line = originLines.get(i % originLines.size());

			if (Constant.DEBUG)
				System.out.println(" line " + i + " angle " + line.angleDegree
						+ " ");

			if (lineMap.get(line) == null)
				continue;

			if (Constant.DEBUG)
				System.out.println(" line " + i % originLines.size()
						+ " in map ");

			int next = 1;
			Line nextLine = originLines.get((i + next) % originLines.size());
			while (lineMap.get(nextLine) == null) {
				next++;
				nextLine = originLines.get((i + next) % originLines.size());
			}

			if (lineMap.get(line).equals(lineMap.get(nextLine))) {
				if (Constant.DEBUG)
					System.out.println("line " + i + " and line " + (i + next)
							+ " is in the same line");

				continue;
			}

			// if two lines are parallel and there are inter lines between these
			// two lines that are not included, probably there should be another
			// line in between
			if (line.isParallel(nextLine) && next != 1) {
				if (Constant.DEBUG)
					System.out.println("line " + i + " and line " + (i + next)
							+ " is parallel");

				int start = (i + 1) % originLines.size();
				int end = (i + next) % originLines.size();

				List<Line> interLines = new ArrayList<Line>();
				if (start > end) {
					interLines.addAll(originLines.subList(start,
							originLines.size()));
					interLines.addAll(originLines.subList(0, end));
				} else
					interLines.addAll(originLines.subList(start, end));

				LineGroup newLineGroup = new LineGroup();
				for (Line l : interLines) {
					newLineGroup.addLine(l);
				}

				// if inter lines length is too big, probably there is another
				// line, so we add only the two neighboring lines into new line
				// group
				if (newLineGroup.length > line.distance(nextLine) * 5) {
					newLineGroup = new LineGroup();
					newLineGroup.addLine(interLines.get(0));
					newLineGroup.addLine(interLines.get(interLines.size() - 1));

					// if the new line is too big, probably should not be a new
					// line added
					if (newLineGroup.length > line.distance(nextLine) * 3)
						continue;
				}

				double lineAngle;
				if (line.isParallel(line.prinAxis)) {
					lineAngle = (90 + line.prinAxis) % 180;
				} else {
					lineAngle = line.prinAxis;
				}

				nextLine = newLineGroup.toLine(lineAngle);

				for (Line interLine : interLines) {
					// change the inter-line angle as it may affect its relation
					// with principle angle
					interLine.angleDegree = lineAngle;
					lineMap.put(interLine, nextLine);
				}

				if (Constant.DEBUG)
					System.out
							.println("line "
									+ i
									+ " and "
									+ (i + next)
									+ " is parallel and new perpendicular line created");
			}

			if (Constant.DEBUG) {
				System.out.println("line " + i + " refined line angle degree "
						+ line.getLineWithAngle(line.prinAxis).angleDegree()
						+ " princ axis " + line.prinAxis);
				System.out.println("next line "
						+ (i + next)
						+ " refined line angle degree "
						+ nextLine.getLineWithAngle(nextLine.prinAxis)
								.angleDegree() + " princ axis "
						+ nextLine.prinAxis);
			}
			Point intersect = line.getLineWithAngle(line.prinAxis).intersect(
					nextLine.getLineWithAngle(nextLine.prinAxis));
			if (intersect != null) {
				if (Constant.DEBUG)
					System.out.println("line " + i + " and " + (i + next)
							+ " intersect");
				conductedPolygon.addPoints(intersect);
			}
		}
		if (conductedPolygon.originPoints == null
				|| conductedPolygon.getPoints().size() <= 2)
			return null;

		if (Constant.DEBUG)
			System.out.println("conducted polygon point size "
					+ conductedPolygon.originPoints.size());

		// generateShrinkedPolygon(fixedHeight, container,
		// conductedPolygon.originPoints);
		return conductedPolygon;
	}

	public List<Polygon> getPolygons() {
		return polygons;
	}

	public KinectContainer getContainer() {
		return container;
	}
}
